﻿using System;
using System.Collections.Generic;
using System.Text;
using RoverStudio.Common.PositionSource;
using System.Drawing;
using RoverStudio.Math;
using WebcamCircleTrackerPositionSource;

namespace Position.CircleTracker
{
    public class CirclePositionSource : IPositionSource
    {
        #region IPositionSource Members

        public event PositionUpdatedHandler PositionUpdated;

        public void Start()
        {
            throw new NotImplementedException();
        }

        public void Stop()
        {
            throw new NotImplementedException();
        }

        #endregion

        private IImageDisplay imageDisplay;


        //Detects the 3-circle pattern on the picture
        private RGCircleTracker rgCircleTracker;


        private IVideoSourceCommon videoSource;

        public IVideoSourceCommon VideoSource
        {
            get { return videoSource; }
            //set { videoSource = value; }
        }


        public CirclePositionSource(IVideoSourceCommon videoSource, IImageDisplay imageDisplay)
        {
            this.imageDisplay = imageDisplay;
            this.videoSource = videoSource;
            this.rgCircleTracker = new RGCircleTracker();

            this.videoSource.NewFrame += new NewFrameDelegate(videoSource_NewFrame);
        }

        private double pixelMeter = 0.02;

        private void videoSource_NewFrame(System.Drawing.Bitmap frame)
        {
            DateTime time = DateTime.Now;
            int x, y;
            double theta;
            Bitmap[] filteredImages;

            //Bitmap input = frame.Clone() as Bitmap;
            this.rgCircleTracker.GetPositionAndDirection(ref frame, out x, out y, out theta, out filteredImages);

            imageDisplay.DisplayImage(filteredImages);
            imageDisplay.SetRealSearchWindow(
                new Rectangle(this.rgCircleTracker.Left,
                    this.rgCircleTracker.Top,
                    this.rgCircleTracker.Width,
                    this.rgCircleTracker.Height));

            Vector3D position = new Vector3D(pixelMeter * (x - frame.Width / 2.0), - pixelMeter * (y - frame.Height / 2.0), 0.0);
            Rotation orientation = new Rotation(new double[,] { { Math.Sin(theta), -Math.Cos(theta), 0.0 }, { Math.Cos(theta), Math.Sin(theta), 0.0 }, { 0.0, 0.0, 1.0 } });

#if DEBUG
            Console.WriteLine("Position: ({0:00.00}; {1:00.00}; {2:00.00})", position.X, position.Y, position.Z);
            Console.WriteLine("Orientation: ({0:00.00}; {1:00.00}; {2:00.00})", orientation.YAxis.X, orientation.YAxis.Y, orientation.YAxis.Z);
#endif
            if (PositionUpdated != null)
                PositionUpdated(position, orientation, time);
        }



        public string OverlayImage
        {
            set
            {
                try
                {
                    Bitmap bitmap = new Bitmap(value);
                    this.rgCircleTracker.OverlayImage = bitmap;
                }
                catch
                {
                    this.rgCircleTracker.OverlayImage = null;
                }
            }
        }


        public byte Threshold
        {
            get
            {
                if (this.rgCircleTracker != null)
                    return this.rgCircleTracker.Threshold;
                else return 0;
            }
            set
            {
                if (this.rgCircleTracker != null)
                    this.rgCircleTracker.Threshold = value;
            }
        }



        public int MinBlobSize
        {
            get
            {
                if (this.rgCircleTracker != null)
                    return this.rgCircleTracker.MinBlobSize;
                else return 0;
            }
            set
            {
                if (this.rgCircleTracker != null)
                    this.rgCircleTracker.MinBlobSize = value;
            }
        }

        public int MaxBlobSize
        {
            get
            {
                if (this.rgCircleTracker != null)
                    return this.rgCircleTracker.MaxBlobSize;
                else return 0;
            }
            set
            {
                if (this.rgCircleTracker != null)
                    this.rgCircleTracker.MaxBlobSize = value;
            }
        }

        public Rectangle SearchWindow
        {
            get
            {
                return new Rectangle(
                    this.rgCircleTracker.Left,
                    this.rgCircleTracker.Top,
                    this.rgCircleTracker.Width,
                    this.rgCircleTracker.Height);

            }
            set
            {
                this.rgCircleTracker.Left = value.Left;
                this.rgCircleTracker.Top = value.Top;
                this.rgCircleTracker.Width = value.Width;
                this.rgCircleTracker.Height = value.Height;

            }
        }

        public bool LockSearchWindow
        {
            get { return this.rgCircleTracker.LockSearchWindow; }
            set { this.rgCircleTracker.LockSearchWindow = value; }
        }


    }
}
